﻿using System;

namespace Robot
{
    struct Vector2
    {
        public double X;
        public double Y;

        public Vector2(double x, double y)
        {
            X = x;
            Y = y;
        }

        public double Length
        {
            get { return Math.Sqrt(X * X + Y * Y); }
        }

        public double LengthSquared
        {
            get { return X * X + Y * Y; }
        }

        public double AngleBetween(Vector2 vector)
        {
            Vector2 vector1 = Normalized;
            Vector2 vector2 = vector.Normalized;

            double ratio = Vector2.DotProduct(vector1, vector2);

            double theta;
            if (ratio < 0)
            {
                theta = Math.PI - 2.0 * Math.Asin((-vector1 - vector2).Length / 2.0);
            }
            else
            {
                theta = 2.0 * Math.Asin((vector1 - vector2).Length / 2.0);
            }
            return theta;
        }

        public static double DistanceSquared(Vector2 a, Vector2 b)
        {
            double dx = a.X - b.X;
            double dy = a.Y - b.Y;
            return dx * dx + dy * dy;
        }

        public static double Distance(Vector2 a, Vector2 b)
        {
            return Math.Sqrt(DistanceSquared(a, b));
        }

        // heading is 0...1
        public static Vector2 GetHeadingVector(double heading)
        {
            double headingRad = 2 * (heading - 0.5) * Math.PI;
            return new Vector2(Math.Sin(headingRad), Math.Cos(headingRad));
        }

        public Vector2 Normalized
        {
            get
            {
                // Computation of length can overflow easily because it
                // first computes squared length, so we first divide by
                // the largest coefficient.
                double x = X;
                double y = Y;
                double m = Math.Abs(x);
                double absy = Math.Abs(y);
                if (absy > m)
                {
                    m = absy;
                }

                x /= m;
                y /= m;

                float length = (float)Math.Sqrt(x * x + y * y);
                return new Vector2(x / length, y / length);
            }
        }
        
        public static double DotProduct(Vector2 a, Vector2 b)
        {
            return a.X * b.X + a.Y * b.Y;
        }

        public static Vector2 Multiply(Vector2 a, double s)
        {
            return new Vector2(a.X * s, a.Y * s);
        }

        public static Vector2 operator +(Vector2 left, Vector2 right)
        {
            return new Vector2(left.X + right.X, left.Y + right.Y);
        }

        public static Vector2 operator -(Vector2 left, Vector2 right)
        {
            return new Vector2(left.X - right.X, left.Y - right.Y);
        }

        public static Vector2 operator -(Vector2 value)
        {
            return new Vector2(-value.X, -value.Y);
        }

        public override string ToString()
        {
            return string.Format("{{{0}; {1}}}", X, Y);
        }

        public double AngleTo(Vector2 position)
        {
            Vector2 toVector = this - position;
            Vector2 upVector = new Vector2(0, 1);
            double heading = toVector.AngleBetween(upVector);
            if (toVector.X < 0)
            {
                heading = -heading;
            }
            return heading;
        }
    }

}
